// Utilities for sensor measurements

// Removes noise from sensor data
int average(int rawIn, int *averageArray) {
  long total = 0;
  static int i;

  i = (i + 1) % 16;
  averageArray[i] = rawIn;
  for (int j = 0; j < 16; j++) total += averageArray[j];
  return total >> 4 ;
}

// Used to average the sensors measurements when everything is level
int findZero(int channel) {
  long zero = 0;
  
  for (int i=0; i< 32; i++) zero += analogRead(channel);
  return zero >> 5;
}

// Allows user to zero gyros on command
void zeroGyros() {
  byte timeout = 0;
  do {
    rollGyroZero = findZero(ROLLRATEPIN);
    timeout++;
  } while ((abs(analogRead(ROLLRATEPIN) - rollGyroZero) > ZEROLIMIT) || (timeout < 100));
  timeout = 0;
  do {
    pitchGyroZero = findZero(PITCHRATEPIN);
    timeout++;
  } while ((abs(analogRead(PITCHRATEPIN) - pitchGyroZero) > ZEROLIMIT) || (timeout < 100));
  timeout = 0;
  do {
    yawGyroZero = findZero(YAWRATEPIN);
    timeout++;
  } while ((abs(analogRead(YAWRATEPIN) - yawGyroZero) > ZEROLIMIT) || (timeout < 100));
}

// Allows user to zero accelerometers on command
void zeroAccelerometers() {
  byte timeout = 0;
  do {
    xAccelZero = findZero(XACCELPIN);
    timeout++;
  } while ((abs(analogRead(XACCELPIN) - xAccelZero) > ZEROLIMIT) || (timeout < 100));
  timeout = 0;
  do {
    yAccelZero = findZero(YACCELPIN);
    timeout++;
  } while ((abs(analogRead(YACCELPIN) - yAccelZero) > ZEROLIMIT) || (timeout < 100));
}

float prFilter(int deltaTime, float levelWeight, float rateGyro, int angleAccel, int zAccel, float *angle) {
  // Original version from http://www.rcgroups.com/forums/showpost.php?p=11085274&postcount=295
  // The weight argument ranges from 0.0 - 1.0.  The lower the number, the more noise is filtered.
  float bias;
  
  // Convert rateGyro from degrees / second into degrees / ms
  // Multiply with elapsed time since last update and integrate (sum it up)
  *angle += (rateGyro / 1000.0) * deltaTime;
  // Calculate the new gyro bias, but let the new value be weighted (%) from the total bias. This filters out some of the noise.
  bias = (bias * (1.0 - levelWeight)) + ((*angle - atan2(angleAccel, zAccel)) * levelWeight);
  return *angle - bias;
}

